use core::u16;
use derive_lib::PeriphReg;
use utils::{ defer, self, ufmt };

const FLASH_KEY1: u32 = 0x45670123;
const FLASH_KEY2: u32 = 0xCDEF89AB;

#[derive(PeriphReg)]
#[periph(periph = "FLASH")]
pub struct FLASH;

#[cfg_attr(feature = "defmt", derive(defmt::Format))]
#[derive(Debug, Clone, Copy, ufmt::derive::uDebug)]
pub enum Error {
    WRPRTERR,
    PGERR,
    VALUE,
}

fn unlock_clear_error() {
    FLASH.keyr.write(|w| w.key().variant(FLASH_KEY1));
    FLASH.keyr.write(|w| w.key().variant(FLASH_KEY2));
    FLASH
        .sr
        .write(|w| w.eop().set_bit().wrprterr().set_bit().pgerr().set_bit());
}

fn lock() {
    FLASH.cr.modify(|_, w| w.lock().set_bit());
}

fn wait_done() -> Result<(), Error> {
    loop {
        let sr = FLASH.sr.read();
        if sr.bsy().bit_is_clear() {
            if sr.wrprterr().bit_is_set() {
                return Err(Error::WRPRTERR);
            } else if sr.pgerr().bit_is_set() {
                return Err(Error::PGERR);
            } else {
                return Ok(());
            }
        }
    }
}

unsafe fn write_u16(addr: u32, value: u16) -> Result<(), Error> {
    FLASH.cr.modify(|_, w| w.pg().set_bit());
    defer!({
        FLASH.cr.modify(|_, w| w.pg().clear_bit());
    });

    let ptr = addr as *mut u16;
    ptr.write_volatile(value);
    wait_done()?;
    if ptr.read_volatile() != value {
        return Err(Error::VALUE);
    }

    Ok(())
}

pub fn write(addr: u32, ptr: *const u8, len: usize) -> Result<(), Error> {
    unlock_clear_error();
    defer!({
        lock();
    });
    
    wait_done()?;

    let mut index = 0;
    while index < len {
        unsafe {
            let n0 = *ptr.add(index);
            let n1 = *ptr.add(index + 1);
            let value = u16::from_ne_bytes([n0, n1]);
            write_u16(addr + (index as u32), value)?;
        }
        index += 2;
    }

    Ok(())
}

#[cfg(any(feature = "md", feature = "hd"))]
fn erase_impl(addr: u32) -> Result<(), Error> {

    FLASH.cr.modify(|_, w| w.per().set_bit());
    defer!({
        FLASH.cr.modify(|_, w| w.per().clear_bit());
    });

    FLASH.ar.write(|w| w.far().variant(addr));

    FLASH.cr.modify(|_, w| w.strt().set_bit());
    defer!({
        FLASH.cr.modify(|_, w| w.strt().clear_bit());
    });

    wait_done()?;

    Ok(())
}

#[cfg(feature = "md")]
const SECTOR_SIZE: usize = 1024;

#[cfg(feature = "hd")]
const SECTOR_SIZE: usize = 2048;

#[cfg(any(feature = "md", feature = "hd"))]
pub fn erase(addr: u32, len: usize) -> Result<(), Error> {
    unlock_clear_error();
    defer!({
        lock();
    });

    let mut index = 0;
    wait_done()?;
    while index < len {
        erase_impl(addr + index as u32)?;
        index += SECTOR_SIZE
    }
    Ok(())
}

pub fn read(addr: u32, ptr: *mut u8, len: usize) {
    unsafe {
        ::core::ptr::copy_nonoverlapping(addr as *const u8, ptr, len);
    }
}
